//OpenCV
#include "cv.h"
#include "highgui.h"
//Other
#include <stdio.h>
#include "string.h"
#include "RGBDCamera_kth.h"
#include "RGBDFrame.cpp"
#include "KeyPointSet.cpp"
#include "KeyPoint.cpp"
#include "FeatureDescriptor.cpp"

#include "FrameMatcher.h"
#include "BasicIcpMatcher.h"
#include "SurfIcpMatcherI.h"

//#include "SurfFeatureDescriptor.cpp"
//#include "myutils.h"
#include "myalgebra.cpp"
#include <sstream>
#include <dirent.h>

using namespace std;

void RGBDCamera_kth::print()
{
	printf("________________________________________________________________________\n");
	printf("storeage_path = %s\n",storeage_path.c_str());
	printf("data_path = %s\n", data_path.c_str());
	printf("nr_words = %i\n", nr_words);
	printf("angle_width = %f\n", angle_width);
	printf("angle_height = %f\n", angle_height);
	printf("mid_width = %f\n", mid_width);
	printf("mid_height = %f\n", mid_height);
	printf("width = %i\n", width);
	printf("height = %i\n", height);
	printf("nr_frames = %i\n", nr_frames);
	printf("current_frame_number = %i\n", current_frame_number);
	printf("________________________________________________________________________\n");
}

void RGBDCamera_kth::store(string path)
{
	printf("store camera:%s\n",path.c_str());
	printf("storeage_path:%s\n",storeage_path.c_str());
	printf("data_path:%s\n",data_path.c_str());
	
	long size_a = 0;
	for(int i = 0; i < nr_words; i++)
	{
		size_a+= words[i]->descriptor_length;
	}
	//printf("size_a = %i\n",size_a);
	
	long size = (3*nr_words+4)*sizeof(int)+size_a*sizeof(float)+storeage_path.length()+data_path.length();
	char * buffer_char 		= new char[size];
	int * buffer_int 		= (int *) buffer_char;
	float * buffer_float 		= (float *) buffer_char;
	//printf("lengths: %i , %i\n",storeage_path.length(),data_path.length());
	int counter = 0;
	buffer_int[counter++]			= storeage_path.length();
	buffer_int[counter++]			= data_path.length();
	buffer_int[counter++]			= current_frame_number;
	buffer_int[counter++]			= nr_words;
	for(int i = 0; i < nr_words; i++)
	{
		buffer_int[counter++]		= words[i]->descriptor_length;
		buffer_int[counter++]		= words[i]->type;
		buffer_int[counter++]		= words[i]->laplacian;
		for(int j = 0; j < words[i]->descriptor_length; j++)
		{
			buffer_float[counter++]	= words[i]->descriptor[j];
		}
	}
	
	for(int i = 0; i < storeage_path.length(); i++)
	{
		buffer_char[counter*4+i] = storeage_path.at(i);
		//printf("%i:%c\n",i,buffer_char[counter*4+i]);
	}
	printf("---\n");
	for(int i = 0; i < data_path.length(); i++)
	{
		buffer_char[counter*4+i+storeage_path.length()] = data_path.at(i);
		//printf("%i:%c\n",i,buffer_char[counter*4+i+storeage_path.length()]);
	}
	//printf("counter: %i\n",counter++);
	ofstream outfile (path.c_str(),ofstream::binary);
	outfile.write (buffer_char,size);
	outfile.close();
	//exit(0);
}

void RGBDCamera_kth::load(string path)
{
	printf("load camera:%s\n",path.c_str());
	ifstream infile (path.c_str(),ifstream::binary);
	infile.seekg(0,ifstream::end);
	long size=infile.tellg();
	char * buffer_char 		= new char [size];
	int * buffer_int 		= (int *) buffer_char;
	float * buffer_float 		= (float *) buffer_char;
	infile.seekg(0);
	infile.read (buffer_char,size);
	infile.close();
	//printf("size: %i l1: %i, l2: %i\n",size,buffer_int[0],buffer_int[1]);
	char buf [1024];
	
	/*char * store_p = new char[buffer_int[0]+1];
	store_p[buffer_int[0]] = 0;
	char * data_p = new char[buffer_int[1]+1];
	data_p[buffer_int[1]] = 0;
	current_frame_number = buffer_int[2];
	*/
	
	int counter = 0;
	int store_len = buffer_int[counter++];
	char * store_p = new char[store_len+1];
	store_p[store_len] = 0;
	
	int data_len = buffer_int[counter++];
	char * data_p = new char[data_len+1];
	data_p[data_len] = 0;
	//print();
	
	int new_current_frame_number = buffer_int[counter++];
	current_frame_number = new_current_frame_number;
	int new_nr_words = buffer_int[counter++];
	nr_words = new_nr_words;
	FeatureDescriptor** new_words = new FeatureDescriptor*[nr_words];
	words = new_words;
	
	//print();
	for(int i = 0; i < nr_words; i++)
	{
		unsigned int desc_len = buffer_int[counter++];
		int desc_type = buffer_int[counter++];
		int desc_lapl = buffer_int[counter++];
		//printf("(%i,%i,%i)\n",desc_len,desc_type,desc_lapl);
		float * desc = new float[desc_len];
		for(int j = 0; j < desc_len; j++)
		{
			desc[j] = buffer_float[counter++];
			//printf("%f\n",desc[j]);
		}
		words[i] = new FeatureDescriptor(desc,desc_len,desc_lapl,desc_type);
	}
	
	for(int i = 0; i < store_len; i++)
	{
		store_p[i] = buffer_char[counter*4+i];
		//printf("%i:%c\n",i,buffer_char[counter*4+i]);
	}
	//printf("---\n");
	for(int i = 0; i < data_len; i++)
	{
		data_p[i] = buffer_char[counter*4+i+store_len];
		//printf("%i:%c\n",i,buffer_char[counter*4+i+buffer_int[0]]);
	}
	delete buffer_char;
	string s1 = string(data_p);
	string s2 = string(store_p); 
	setup(s1,s2);
	current_frame_number = new_current_frame_number;
	nr_words = new_nr_words;
	words = new_words;
}

void RGBDCamera_kth::setup(string datapath, string storepath)
{
	words = 0;
	nr_words = 0;
	initialize_constants();
	printf("new camera\n");
	data_path = datapath;
	storeage_path = storepath;
	current_frame_number = 0;
	printf("data found at %s\n",data_path.c_str());
	printf("store found at %s\n",storeage_path.c_str());
	
	//Camera paramters
	angle_width  	= pi*57.0/360.0;
	angle_height 	= pi*43.0/360.0;
	width 			= 640;
	height 			= 480;
	mid_width    	= width/2.0;
	mid_height   	= height/2.0;
	
	transformation_matrixes=0;
	setup_full_transformation_matrix();
	
	
	DIR *dir;
	struct dirent *ent;
    dir = opendir (data_path.c_str());
    files_in_dir = 0;
	if (dir != NULL) {
		while ((ent = readdir (dir)) != NULL) {files_in_dir++;}
		closedir (dir);
	} else {printf("no directory to load data from: exiting!\n");exit(-1);}
	printf("total %i files\n",files_in_dir);
	rgb_list   = new string*[files_in_dir];
	depth_list = new string*[files_in_dir];
	timings = new double[files_in_dir];
	for(int i = 0; i < files_in_dir; i++)
	{
		rgb_list[i] = 0;
		depth_list[i] = 0;
	}

    dir = opendir (data_path.c_str());
	while ((ent = readdir (dir)) != NULL) {
		string name = ent->d_name;
		int strlen = name.length();
		size_t pos = name.find("_");
		if((int)pos >= 0){;
			string str3 = name.substr(pos+1);
			pos = str3.find("_");
			string str4 = str3.substr(0,pos);
			istringstream buffer(str4);
			int frame_number;
			buffer >> frame_number;
			pos = name.find("rgb");
			bool is_rgb = (int)pos > 0;
			string str5;
			if(is_rgb)
			{
				rgb_list[frame_number] = new string(data_path+name);
				str5 = name.substr(name.find("rgb")+4);
			}
			else
			{
				depth_list[frame_number] = new string(data_path+name);
				str5 = name.substr(name.find("depth")+6);
			}
			pos = str5.find("_");
			string str6 = str5.substr(0,pos);
			string str7 = str5.substr(pos+1,str5.find(".")-pos-1);
			istringstream buffer2(str6);
			int timing_large;
			buffer2 >> timing_large;
			
			istringstream buffer3(str7);
			int timing_small;
			buffer3 >> timing_small;
			double timing_tmp = timing_small;
			while(timing_tmp > 1)
			{
				timing_tmp/=10;
			}
			timing_tmp+=timing_large;

			timings[frame_number] = timing_tmp;
		}
	}
	closedir (dir);
	int true_pos = 0;
	for(int i = 0; i < files_in_dir;i++)
	{
		if(rgb_list[i]!=0)
		{
			rgb_list[true_pos] = rgb_list[i];
			depth_list[true_pos] = depth_list[i];
			timings[true_pos] = timings[i];
			//printf("timing: %f\n",timings[true_pos]);
			true_pos++;
		}
	}
	nr_frames = true_pos;
	for(int i = true_pos; i < files_in_dir;i++)
	{
			rgb_list[i] = 0;
			depth_list[i] = 0;
	}
}

RGBDCamera_kth::RGBDCamera_kth(){}

RGBDCamera_kth::RGBDCamera_kth(string datapath, string storepath)
{
	setup(datapath, storepath);
}


RGBDCamera_kth::~RGBDCamera_kth()
{

	for(int i = 0; i < width; i++)
	{
		for(int j = 0; j < height; j++)
		{
			for(int k = 0; k < 4; k++)
			{
				delete transformation_matrixes[i][j][k];
			}
			delete transformation_matrixes[i][j];
		}
		delete transformation_matrixes[i];
	}
	delete transformation_matrixes;
	for(int i = 0; i < files_in_dir; i++)
	{
		if(rgb_list[i]!=0){delete rgb_list[i];}
		if(depth_list[i]!=0){delete depth_list[i];}
	}
	delete rgb_list;
	delete depth_list;
	delete timings;

}

/*
void RGBDCamera_kth::setCodebook(Codebook * codebook_pos, Codebook * codebook_neg){
	printf("setting codebooks\n");
	
	nr_words = codebook_pos->getCodebookSize()+codebook_neg->getCodebookSize();
	printf("nr_words = %i\n",nr_words);

	words = new FeatureDescriptor*[nr_words];//new float*[nr_words];
	for(int i = 0; i < codebook_pos->getCodebookSize(); i++)
	{
		float * this_word = new float[128];
		double * descr = codebook_pos->getCenter(i).getRawData();
		for(int j = 0; j < 128; j++){this_word[j] = descr[j];}
		words[i] = new FeatureDescriptor(this_word,128,1,1);
	}
	for(int i = 0; i < codebook_neg->getCodebookSize(); i++)
	{
		float * this_word = new float[128];
		double * descr = codebook_neg->getCenter(i).getRawData();
		for(int j = 0; j < 128; j++){this_word[j] = descr[j];}
		words[codebook_pos->getCodebookSize()+i] = new FeatureDescriptor(this_word,128,-1,1);
	}
	
}
*/
void RGBDCamera_kth::setup_full_transformation_matrix()
{
	printf("setting up transformation matrix\n");
	double width_d = width;
	double height_d = height;
	
	//width rotations
	double tan_w_angle = tan(angle_width);
	double *** ROTx_arr = new double**[width];
	for(double w = 0.5; w < width; w++)
	{
		double angle_w = atan(tan_w_angle*2*(w-mid_width)/(width_d-1))/2;
		double ** ROTx = new double*[4];
		for(int i = 0; i < 4; i++)
		{
			ROTx[i] = new double[4];
		}
		double cosx = cos(angle_w);
		double sinx = sin(angle_w);
		ROTx_arr[(int)w] = ROTx;
		ROTx[0][0] = 1;
		ROTx[0][1] = 0;
		ROTx[0][2] = 0;
		ROTx[0][3] = 0;
		
		ROTx[1][0] = 0;
		ROTx[1][1] = cosx;
		ROTx[1][2] = -sinx;
		ROTx[1][3] = 0;
		
		ROTx[2][0] = 0;
		ROTx[2][1] = sinx;
		ROTx[2][2] = cosx;
		ROTx[2][3] = 0;
		
		ROTx[3][0] = 0;
		ROTx[3][1] = 0;
		ROTx[3][2] = 0;
		ROTx[3][3] = 1;
	}
	//height rotations
	double tan_h_angle = tan(angle_height);
	double *** ROTy_arr = new double**[height];
	for(double h = 0.5; h < height; h++)
	{
		double angle_h = atan(tan_h_angle*2*(h-mid_height)/(height_d-1))/2;
		double ** ROTy = new double*[4];
		for(int i = 0; i < 4; i++)
		{
			ROTy[i] = new double[4];
		}
		
		double cosy = cos(angle_h);
		double siny = sin(angle_h);
		
		ROTy_arr[(int)h] = ROTy;
		
		ROTy[0][0] = cosy;
		ROTy[0][1] = 0;
		ROTy[0][2] = siny;
		ROTy[0][3] = 0;
		
		ROTy[1][0] = 0;
		ROTy[1][1] = 1;
		ROTy[1][2] = 0;
		ROTy[1][3] = 0;
		
		ROTy[2][0] = -siny;
		ROTy[2][1] = 0;
		ROTy[2][2] = cosy;
		ROTy[2][3] = 0;
		
		ROTy[3][0] = 0;
		ROTy[3][1] = 0;
		ROTy[3][2] = 0;
		ROTy[3][3] = 1;
		
	}
	
	transformation_matrixes = new double***[width];	
	for(int w = 0; w < width; w++)
	{
		transformation_matrixes[w] = new double**[height];
		for(int h = 0; h < height; h++)
		{
			double ** transformation = new double*[4];
			for(int i = 0; i < 4; i++)
			{
				transformation[i] = new double[4];
			}
			matmul(ROTx_arr[w], 4, 4,ROTy_arr[h], 4, 4, transformation);
			transformation_matrixes[w][h] = transformation;
		}
	}
	
	
	for(int w = 0; w < width; w++)
	{
		for(int i = 0;i<4;i++)
		{
			delete ROTx_arr[w][i];
		}
		delete ROTx_arr[w];
	}
	delete ROTx_arr;
	
	for(int h = 0; h < height; h++)
	{
		for(int i = 0;i<4;i++)
		{
			delete ROTy_arr[h][i];
		}
		delete ROTy_arr[h];
	}
	delete ROTy_arr;
}


RGBDFrame * RGBDCamera_kth::getFrame(int i)
{
	//printf("getting frame %i\n");
	if(i < 0 || i >= nr_frames){return NULL;}
	return new RGBDFrame(this);
}

RGBDFrame * RGBDCamera_kth::getNextFrame()
{
	return getFrame(current_frame_number++);
}

RGBDFrame * RGBDCamera_kth::getCurrentFrame()
{
	return getFrame(current_frame_number);
}

RGBDFrame * RGBDCamera_kth::getPreviousFrame()
{
	return getFrame(current_frame_number--);
}

